#include <stdio.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"

#include "iot_pwm.h"
#include "iot_gpio.h"

#include "hi_types_base.h"
#include "hi_early_debug.h"
#include "hi_pwm.h"
#include "hi_gpio.h"
#include "hi_io.h"

#include "car_test.h"

//左前轮: 前进后退控制GPIO口[低电平有效]
#define Wheels_Left_Front_Forward_Ctrl   (0)
#define Wheels_Left_Front_Backward_Ctrl  (1)
//左后轮: 前进后退控制GPIO口[低电平有效]
#define Wheels_Left_Back_Forward_Ctrl    (6)
#define Wheels_Left_Back_Backward_Ctrl   (8)
//右前轮: 前进后退控制GPIO口[低电平有效]
#define Wheels_Right_Front_Forward_Ctrl  (10)
#define Wheels_Right_Front_Backward_Ctrl (9)
//右后轮: 前进后退控制GPIO口[低电平有效]
#define Wheels_Right_Back_Forward_Ctrl   (14)
#define Wheels_Right_Back_Backward_Ctrl  (13)

// 右边小板电机1:J3[B0,A0]        ==>> 左前轮: 前进后退控制GPIO口
#define DJ_1_GPIOA   Wheels_Left_Front_Forward_Ctrl
#define DJ_1_GPIOB   Wheels_Left_Front_Backward_Ctrl
// 右边小板电机2:J10[B0,A0] ==>> 右前轮: 前进后退控制GPIO口
#define DJ_2_GPIOA   Wheels_Right_Front_Backward_Ctrl
#define DJ_2_GPIOB   Wheels_Right_Front_Forward_Ctrl

#define DOUBLE_ELECTRIC_MACHINERY  // 3.3V + 5V
#if defined(DOUBLE_ELECTRIC_MACHINERY)
// 左边小板电机1:J3[B0,A0]         ==>> 左后轮: 前进后退控制GPIO口
#define DJ_3_GPIOB   Wheels_Left_Back_Forward_Ctrl
#define DJ_3_GPIOA   Wheels_Left_Back_Backward_Ctrl
// 左边小板电机1:J10[B0,A0] ==>> 右后轮: 前进后退控制GPIO口
#define DJ_4_GPIOA   Wheels_Right_Back_Backward_Ctrl
#define DJ_4_GPIOB   Wheels_Right_Back_Forward_Ctrl
#endif

#ifdef CAR_DISPLAY_CMD
extern void car_cmd_test(char* str);
#endif
void gpio_control(unsigned int gpio, IotGpioValue value) {
    // 控制IO口输出
    hi_io_set_func(gpio, 0);
#if defined(DOUBLE_ELECTRIC_MACHINERY)
    hi_io_set_func(13, 4);
    hi_io_set_func(14, 4);
#endif
    IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(gpio, value);
}

struct car_sys_info car_info;
// CarStatus carstatus = CAR_STATUS_STOP;
// CarMode carmode = CAR_MODE_STEP;

void car_info_init(void)
{
    car_info.go_status = CAR_STATUS_STOP;
    car_info.cur_status = CAR_STATUS_STOP;

    car_info.mode = CAR_MODE_STEP;
    car_info.protocol = COMM_PROTOCOL_UDP;

    car_info.step_count = CAR_STEP_COUNT;
}

void step_count_update(void)
{
    car_info.step_count = CAR_STEP_COUNT;
}

void set_car_status(CarStatus status)
{
    if(status != car_info.cur_status)
    {
        car_info.status_change = 1;
    }
    car_info.go_status = status;

    step_count_update();
}


//设置行驶模式
void set_car_mode(CarMode mode)
{
    car_info.mode = mode;
}

//设置通信协议
void set_comm_mode(ProtoStatus mode)
{
    car_info.protocol = mode;
}

void car_gpio_init(void)
{
    // IO口初始化
    IoTGpioInit(DJ_1_GPIOA);
    IoTGpioInit(DJ_1_GPIOB);
    IoTGpioInit(DJ_2_GPIOA);
    IoTGpioInit(DJ_2_GPIOB);
#if defined(DOUBLE_ELECTRIC_MACHINERY)
    IoTGpioInit(DJ_3_GPIOA);
    IoTGpioInit(DJ_3_GPIOB);
    IoTGpioInit(DJ_4_GPIOA);
    IoTGpioInit(DJ_4_GPIOB);
#endif
    printf("car_gpio_init \r\n");
}

// 停止
void car_gpio_stop(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE1);
    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE1);
    gpio_control(DJ_3_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_3_GPIOB, IOT_GPIO_VALUE1);
    gpio_control(DJ_4_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_4_GPIOB, IOT_GPIO_VALUE1);
// #else
//  gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE1);
//  gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE1);
//  gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE1);
//  gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE1);
#endif

}
void car_stop(void)
{
    car_info.cur_status = car_info.go_status;

    printf("car_gpio_stop \r\n");

    car_gpio_stop();
}

//前进
void car_gpio_forward(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE0);
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE1);
    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE0);
    gpio_control(DJ_3_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_3_GPIOB, IOT_GPIO_VALUE0);
    gpio_control(DJ_4_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_4_GPIOB, IOT_GPIO_VALUE0);
#else
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE0);
    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE0);
#endif
}
void car_forward(void)
{
    if(car_info.go_status != CAR_STATUS_FORWARD)
    {
        //直接退?
        return;
    }
    if(car_info.cur_status == car_info.go_status)
    {
        //状态没有变化，直接推出
        return;
    }

    car_info.cur_status = car_info.go_status;

    printf("car_gpio_forward \r\n");

    car_gpio_forward();

    step_count_update();
}

//后退
void car_gpio_backward(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE0);
    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE0);
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE1);
    gpio_control(DJ_3_GPIOA, IOT_GPIO_VALUE0);
    gpio_control(DJ_3_GPIOB, IOT_GPIO_VALUE1);
    gpio_control(DJ_4_GPIOA, IOT_GPIO_VALUE0);
    gpio_control(DJ_4_GPIOB, IOT_GPIO_VALUE1);
#else
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE0);
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE1);
    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE0);
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE1);
#endif
}
void car_backward(void)
{
    if(car_info.go_status != CAR_STATUS_BACKWARD)
    {
        //直接退?
        return ;
    }
    if(car_info.cur_status == car_info.go_status)
    {
        //状态没有变化，直接推出
        return;
    }

    car_info.cur_status = car_info.go_status;

    printf("car_gpio_backward \r\n");

    car_gpio_backward();

    step_count_update();
}

//左转
void car_gpio_left(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE0);  //Wheels_Left_Front_Forward_Ctrl[0]
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE1);  //Wheels_Left_Front_Backward_Ctrl[1]--

    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE1);  //Wheels_Right_Front_Backward_Ctrl[1]--
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE0);  //Wheels_Right_Front_Forward_Ctrl[0]

    gpio_control(DJ_3_GPIOA, IOT_GPIO_VALUE1);  //Wheels_Left_Back_Backward_Ctrl[1]--
    gpio_control(DJ_3_GPIOB, IOT_GPIO_VALUE0);  //Wheels_Left_Back_Forward_Ctrl[0]

    gpio_control(DJ_4_GPIOA, IOT_GPIO_VALUE1);  //Wheels_Right_Back_Backward_Ctrl[1]--
    gpio_control(DJ_4_GPIOB, IOT_GPIO_VALUE1);  //Wheels_Right_Back_Forward_Ctrl[1]--
#else
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE0);
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE0);
    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE0);
#endif
}
void car_left(void)
{
    if(car_info.go_status != CAR_STATUS_LEFT)
    {
        //直接退出?
        return ;
    }
    if(car_info.cur_status == car_info.go_status)
    {
        //状态没有变化，直接推出
        return;
    }

    car_info.cur_status = car_info.go_status;

    printf("car_gpio_left \r\n");

    car_gpio_left();

    step_count_update();
}

//右转
void car_gpio_right(void)
{
#if defined(DOUBLE_ELECTRIC_MACHINERY)
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE0);  //Wheels_Left_Front_Forward_Ctrl[0]
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE1);  //Wheels_Left_Front_Backward_Ctrl[1]--

    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE1);  //Wheels_Right_Front_Backward_Ctrl[1]--
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE0);  //Wheels_Right_Front_Forward_Ctrl[0]

    gpio_control(DJ_3_GPIOA, IOT_GPIO_VALUE1);  //Wheels_Left_Back_Backward_Ctrl[1]--
    gpio_control(DJ_3_GPIOB, IOT_GPIO_VALUE1);  //Wheels_Left_Back_Forward_Ctrl[1]--

    gpio_control(DJ_4_GPIOA, IOT_GPIO_VALUE1);  //Wheels_Right_Back_Backward_Ctrl[1]--
    gpio_control(DJ_4_GPIOB, IOT_GPIO_VALUE0);  //Wheels_Right_Back_Forward_Ctrl[0]
#else
    gpio_control(DJ_1_GPIOA, IOT_GPIO_VALUE1);
    gpio_control(DJ_1_GPIOB, IOT_GPIO_VALUE0);
    gpio_control(DJ_2_GPIOA, IOT_GPIO_VALUE0);
    gpio_control(DJ_2_GPIOB, IOT_GPIO_VALUE0);
#endif
}
void car_right(void)
{
    if(car_info.go_status != CAR_STATUS_RIGHT)
    {
        //直接退出?
        return ;
    }
    if(car_info.cur_status == car_info.go_status)
    {
        //状态没有变化，直接推出
        return;
    }

    car_info.cur_status = car_info.go_status;

    printf("car_gpio_right \r\n");

    car_gpio_right();

    step_count_update();
}

extern void start_tcp_thread(void);
extern void start_udp_thread(void);
extern void cancel_tcp_thread(void);
extern void cancel_udp_thread(void);

void car_test(void)
{
    start_tcp_thread();
    car_gpio_init();
    car_info_init();
    //set_car_status(CAR_STATUS_FORWARD);
    //set_car_mode(CAR_MODE_ALWAY);

    car_forward();
    sleep(1);

    car_backward();
    sleep(1);

    car_right();
    sleep(1);

    car_left();
    sleep(1);

    car_stop();

    while(1)
    {
        if(car_info.status_change)
        {
            car_info.status_change = 0;

            switch(car_info.go_status)
            {
                case CAR_STATUS_STOP:
                    car_stop();
                    break;

                case CAR_STATUS_FORWARD:
                    car_forward();
                    break;

                case CAR_STATUS_BACKWARD:
                    car_backward();
                    break;

                case CAR_STATUS_LEFT:
                    car_left();
                    break;

                case CAR_STATUS_RIGHT:
                    car_right();
                    break;

                default:
                    break;
            }
        }

        if(car_info.mode == CAR_MODE_STEP)
        {
            if(car_info.go_status != CAR_STATUS_STOP)
            {
                if(car_info.step_count > 0)
                {
                    car_info.step_count --;
                    printf("car_info.step_count = %d \r\n",car_info.step_count);
                }
                else
                {
                    printf("stop... \r\n");
                    set_car_status(CAR_STATUS_STOP);
                }
            }
        }

        // if (car_info.protocol == COMM_PROTOCOL_TCP)
        // {
        //     cancel_udp_thread();
        //     printf("======== 【UDP】 Stop Protocol========\r\n");
        //     start_tcp_thread();
        //     printf("======== 【TCP】 Start Protocol ========\r\n");
        //     car_info.protocol = COMM_PROTOCOL_NOP;
        // }
        // else if (car_info.protocol == COMM_PROTOCOL_UDP)
        // {
        //     cancel_tcp_thread();
        //     printf("======== 【TCP】 Stop Protocol ========\r\n");
        //     start_udp_thread();
        //     printf("======== 【UDP】 Start Protocol ========\r\n");
        //     car_info.protocol = COMM_PROTOCOL_NOP;
        // }
        // else {
        //     car_info.protocol = COMM_PROTOCOL_NOP;
        // }

        usleep(1000);
    }

}

#if 0  //for test use only
// to confirm which gpio control which wheel
void CarTask(void)
{
    printf("CarTask: Begin\n");
    int i = 0;
    // 0/1, 6/8, 9/10, 13/14
    char table[15] = {1, 1, 0,
                      0, 0, 0, 1,  //3+4: uart0; 5+6: uart1
                      0, 1, 1, 1,
                      0, 0, 1, 1,  //11+12: uart2
                     };

    for (i = 0; i < 15; i++) {
        if( table[i] )
            gpio_control(i, 1);  //set Hi to disable GPIOi
    }

    i = 0;
    while (i < 15) {
        if( table[i] ) {
            printf("==============[%d]==============\n", i);
            printf("CarTask: GPIO[%d] set[0]\n", i);
            IoTGpioSetOutputVal(i, 0);
            sleep(3);
            printf("CarTask: GPIO[%d] set[1]\n", i);
            IoTGpioSetOutputVal(i, 1);
            sleep(1);
            printf("CarTask: GPIO[%d] set[0]\n", i);
            IoTGpioSetOutputVal(i, 0);
            sleep(3);
            printf("CarTask: GPIO[%d] set[1]\n", i);
            IoTGpioSetOutputVal(i, 1);
            sleep(1);
            printf("CarTask: GPIO[%d] set[0]\n", i);
            IoTGpioSetOutputVal(i, 0);
            sleep(3);
            printf("CarTask: GPIO[%d] set[1]\n", i);
            IoTGpioSetOutputVal(i, 1);

            sleep(3);
        }
        i = (++i) % 15;
    }
    printf("CarTask: End\n");
}

static void CarTaskEntry(void)
{
    osThreadAttr_t attr;

    attr.name = "CarTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 2048;
    attr.priority = 25;
    if(osThreadNew((osThreadFunc_t)CarTask, NULL, &attr) == NULL) {
        printf("[CarTaskEntry] Faild to create CarTask!\n");
    }
}
SYS_RUN(CarTaskEntry);
#endif